package behavior;
import java.util.ArrayList;

import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.UltrasonicSensor;
import lejos.robotics.navigation.TachoPilot;
import Interface.BluetoothNXT;


public class Panic {	
	Motor sensorMotor = Motor.A;
	final boolean VERBOSE = true;
	public final int goalDistance = 38;	
	private final int getAngleDist = 15;
	
	private TachoPilot pilot;
	private UltrasonicSensor sonicSensor;
	private BluetoothNXT bluetooth;
	
	public Panic(TachoPilot pilot, UltrasonicSensor sonicSensor, BluetoothNXT bluetooth){		
		this.pilot = pilot;
		this.sonicSensor = sonicSensor;
		this.bluetooth = bluetooth;
	}
	
	public Panic(){
		this.pilot = new TachoPilot((float)5.43, (float)5.43, (float)16.18, Motor.B, Motor.C, true);
		sonicSensor = new UltrasonicSensor(SensorPort.S2);
	}
	
	@SuppressWarnings("unchecked")
	public void action() throws InterruptedException{			
		sonicSensor.getDistance();//Herp Ik ben een sonicSensor Derp		
			
		pilot.travel(-30);		
		ArrayList<Object> results = checkSurroundings();
		int distanceClosestToGoal = (Integer) results.get(0);
		int rotationClosestToGoal = (Integer) results.get(1);
		ArrayList<Integer> rightDistances = (ArrayList<Integer>) results.get(2);
		ArrayList<Integer> leftDistances = (ArrayList<Integer>) results.get(3);
		
		int correctionAngle = calculateCorrectionAngle(rotationClosestToGoal, distanceClosestToGoal);		
		
		if (bluetooth != null) sendCollectedDistances(rightDistances, leftDistances, bluetooth);		
		
		float angleFromParallel = correctPosition(distanceClosestToGoal, rotationClosestToGoal, correctionAngle);
		ArrayList<Integer> distances = analyseWalls();
		getInMiddle(distances);
		
		if(VERBOSE){
			LCD.clear();
			LCD.drawString("dist:" + distanceClosestToGoal,0,0);
			LCD.drawString("angle:" + rotationClosestToGoal,0,1);
			LCD.drawString("corrA:" + correctionAngle,0,2);	
			LCD.drawString("corrDist:" + (goalDistance-distanceClosestToGoal)*10,0,3);
			LCD.drawString("Par:" + angleFromParallel,0,4);
			Button.waitForPress();
		}
	}

	public float correctPosition(int distanceClosestToGoal, int rotationClosestToGoal, int correctionAngle){
		sensorMotor.rotate(180, true);
		pilot.rotate(correctionAngle);
		pilot.travel((goalDistance-distanceClosestToGoal));
		if(rotationClosestToGoal >270){
			pilot.rotate(-90);
		}else {
			pilot.rotate(90);
		}
		if(rotationClosestToGoal < 180){
			float angleFromParallel = getAngle(true, getAngleDist); //Kijk naar links
			pilot.rotate(angleFromParallel);
			pilot.travel(-5);
			return angleFromParallel;
		}else{
			float angleFromParallel = getAngle(false, getAngleDist);
			pilot.rotate(angleFromParallel);
			pilot.travel(-5);
			return angleFromParallel;
		}		
	}	
	
	public ArrayList<Object> checkSurroundings() throws InterruptedException{
		int distanceClosestToGoal = Integer.MAX_VALUE;
		int rotationClosestToGoal = -1;		
		int distance = -1;
		
		ArrayList<Integer> rightDistances = new ArrayList<Integer>();
		for(int i = 0; i < 18; i++){
			distance = sonicSensor.getDistance();Thread.sleep(75);
			rightDistances.add(distance);
			if(distance < distanceClosestToGoal){
				distanceClosestToGoal = distance;
				rotationClosestToGoal = (i)*10;
			}
			sensorMotor.rotate(10);
		}
		
		ArrayList<Integer> leftDistances = new ArrayList<Integer>();
		sensorMotor.rotate(-180);
		for(int i = 0; i < 18; i++){
			sensorMotor.rotate(-10);Thread.sleep(75);
			distance = sonicSensor.getDistance();
			leftDistances.add(distance);
			if(distance < distanceClosestToGoal){
				distanceClosestToGoal = distance;
				rotationClosestToGoal = 360-((i+1)*10);
			}
		}
		ArrayList<Object> results = new ArrayList<Object>();
		results.add(distanceClosestToGoal); //index 0
		results.add(rotationClosestToGoal); //index 1
		results.add(rightDistances); //index 2
		results.add(leftDistances); //index 3
		
		return results;
	}
	
	public int calculateCorrectionAngle(int rotationClosestToGoal, int distanceClosestToGoal) throws InterruptedException{
		int correctionAngle = -1;
		if(rotationClosestToGoal >270){
			correctionAngle = rotationClosestToGoal-180;
		}else if (rotationClosestToGoal <= 90){
			correctionAngle =-(180-rotationClosestToGoal);
		}else{
			correctionAngle =  -(270-rotationClosestToGoal);
			sensorMotor.rotate(90, true);
			pilot.travel(5);//Beetje naar voor, anders zal draaien moeilijk worden wss
			Thread.sleep(75);
			int distance = sonicSensor.getDistance();
			if(distance < distanceClosestToGoal){
				distanceClosestToGoal = distance;
			}
			Thread.sleep(75);
			sensorMotor.rotate(-90);
		}
		return correctionAngle;
	}
	
	public void sendCollectedDistances(ArrayList<Integer> rightDistances, ArrayList<Integer> leftDistances, BluetoothNXT bluetooth){
		ArrayList<Integer> distances = new ArrayList<Integer>();
		distances.addAll(leftDistances);
		reverse(rightDistances);
		distances.addAll(rightDistances);
		bluetooth.sendIntegers(distances);
	}
	
	/**
	 * Returns the angle from a parallel position (in degrees)
	 */
	public float getAngle(boolean links, int dist){		
		int sign = links ? 1 : -1;
		LCD.drawString("sign:" + sign, 0, 4);
		sensorMotor.rotate(sign*90);
		int distance1= readValue();
		LCD.drawString("dist1:" + distance1,0,0);
		pilot.travel(dist);
		int distance2= readValue();
		LCD.drawString("dist2:" + distance2,0,1);
		sensorMotor.rotate(sign*-90);
		pilot.travel(-dist);
		double v = Math.abs(distance1-distance2);
		float angle = (float) Math.atan(v/dist); //Nog in radians	
		angle *= 180/Math.PI; //Omzetten naar graden
		if(distance1 > distance2){
			angle *= -1;		//Juiste kant draaien afh van links of rechts kijken
		}
		return sign*angle;
	}
	
	public void getInMiddle(ArrayList<Integer> distances){
		int left = distances.get(0);
		int right = distances.get(2);
		if(left < 40){
			pilot.rotate(90);
			pilot.travel(-(37-left));
			pilot.rotate(-90);
		}
		else if(left>40 && left <70){
			pilot.rotate(-90);
			pilot.travel(-(left-37));
			pilot.rotate(90);
		}
		else if(right<40){
			pilot.rotate(-90);
			pilot.travel(-(37-right));
			pilot.rotate(90);
		}
		else if(right>40&& right<70){
			pilot.rotate(90);
			pilot.travel(-(-37+right));
			pilot.rotate(-90);
		}
		int forward = distances.get(1);
		if(forward>40&&forward<70)
			pilot.travel(forward-37);
		else if(forward<40)
			pilot.travel(37-forward);  
	}
	
	public ArrayList<Integer> analyseWalls() throws InterruptedException{
		sonicSensor.getDistance(); //Hurr Ima sonicsensor Durr
		Thread.sleep(1000);
		ArrayList<Integer> distances = new ArrayList<Integer>(); 
		//getSonicSensor().getDistance()
		sensorMotor.rotate(90); 
		distances.add(readValue());
		sensorMotor.rotate(-90); 
		distances.add(readValue());
		sensorMotor.rotate(-90); 
		distances.add(readValue());
		LCD.clear();
		LCD.drawString("Links:" + distances.get(0), 0, 1);
		LCD.drawString("Voor:" + distances.get(1), 0, 2);
		LCD.drawString("Rechts:" + distances.get(2), 0, 3);
		sensorMotor.rotate(90);
		return distances;
	}
//	
//	public BluetoothNXT connectToBluetooth() {
//		BluetoothNXT bluetooth = null;
//		bluetooth = new BluetoothNXT(pilot);
//		bluetooth.start();		
//		return bluetooth;
//	}
	public static void reverse( ArrayList<Integer> rightDistances ){
	   for ( int start = 0, end = rightDistances.size() - 1 ; start < rightDistances.size() / 2; start++, end-- ) {
	      swap( rightDistances, start, end ) ;
	   }
	}	
	
	private static void swap( ArrayList<Integer> rightDistances, int front, int back ){
	   int temp = rightDistances.set( front, rightDistances.get( back ) ) ;
	   rightDistances.set( back, temp) ;
	}
	
	public int readValue(){
		ArrayList<Integer> vals = new ArrayList<Integer>();
		for(int i = 0; i < 100; i++){
			vals.add(sonicSensor.getDistance());
		}
		int finalValue = vals.get(0);
		for(int value : vals){
			if (value != 255) finalValue = value;
		}
		return finalValue;
	}


}